# @Time   : 2022/3/29 14:50
# @Author : Fioman
# @Phone  : 13149920693
# @Tips   : Talk is Cheap,Show me the code! ^_^^_^
"""
轮廓查找新算法,物料区域是白色情况.思路就是先找到上下的轮廓,然后找中间的额黑色图像.黑色图像查找之前要先将轮廓定义处理一下.找到最大的那个.
"""

# @Time   : 2022/3/29 9:52
# @Author : Fioman
# @Phone  : 13149920693
# @Tips   : Talk is Cheap,Show me the code! ^_^^_^


import time
import imutils
from tools.config_params import ConfigParams
from vision_process.image_transfer import *
from settings import *

areaFilterUpDown = 500
areaFilterLeftRight = 8000
cp = ConfigParams()
colorBlue = (255, 0, 0)
colorGreen = (0, 255, 0)
colorRed = (0, 0, 255)
lineThickness = 5
colorList = [colorBlue, colorGreen, colorRed]

minLineLength = int(30 * cp.mm2pix)  # 线条最短是多少,50个mm
lineLengthMin = 25 * cp.mm2pix
maxLineGap = int(5 * cp.mm2pix)


def image_show(name, image, showState=False):
    """
    显示图片,用来调试的时候显示图片,按照一定的比例进行缩放
    :param name:显示的图片窗口名称
    :param image:显示的图片
    :param showState:是否显示
    :return:
    """
    if showState:
        sizeScale = 4
        newWidth = int(image.shape[1] / sizeScale)
        newHeight = int(image.shape[0] / sizeScale)
        showImage = cv.resize(image, (newWidth, newHeight), cv.INTER_AREA)
        cv.namedWindow("{}".format(name))
        cv.imshow("{}".format(name), showImage)
        cv.waitKey(0)


def get_size_scale_image(imageSrc, sizeScale):
    """
    高宽同等比例缩放图像,缩放比例
    :param imageSrc: 原图像
    :param sizeScale: 缩放比例
    :return:
    """
    h, w = imageSrc.shape[:2]
    newWidth, newHeight = int(w / sizeScale), int(h / sizeScale)
    newImage = cv.resize(imageSrc, (newWidth, newHeight), cv.INTER_AREA)
    return newImage


def get_center_by_contour(contour):
    """
    根据轮廓点获取这个轮廓的中心点
    :param contour:
    :return:
    """
    return int(cv.minAreaRect(contour)[0][0]), int(cv.minAreaRect(contour)[0][1])


def get_two_points_middle(p1, p2):
    """
    获取两个点的中点坐标
    :param p1: 点1
    :param p2: 点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    middleX = (p1[0] + p2[0]) / 2
    middleY = (p1[1] + p2[1]) / 2
    return round(middleX, 2), round(middleY, 2)


def get_line_k_and_b(p1, p2):
    """
    湖区某一条直线的斜率和斜距
    :param p1: 点1的坐标
    :param p2: 点2的坐标
    :return:
    """
    if p1[0] == p2[0]:
        # 垂直于x轴,没有k值
        return None, p1[0]
    else:
        k = (p2[1] - p1[1]) / (p2[0] - p1[0])
        b = p1[1] - k * p1[0]
        return round(k, 2), b


def get_two_lines_intersection(k1, b1, k2, b2):
    """
    获取平面上两条线的交点
    :param k1: 线1的k值
    :param b1: 线1的b值
    :param k2: 线2的k值
    :param b2: 线2的b值
    :return:
    """
    # 如果k相等,说明垂直无交点
    if k1 == k2:
        return None, None
    elif k1 is None and k2 is not None:
        return int(b1), int(k2 * b1 + b2)
    elif k2 is None and k1 is not None:
        return int(b2), int(k1 * b2 + b1)
    else:
        x = (b1 - b2) / (k1 - k2) * (-1)
        y = k1 * x + b1
    return int(x), int(y)


def get_rect_points_clockwise(rectPoints):
    """
    根据矩形的四个顶点,获取其顺时针方向的四个顶点
    :param rectPoints:
    :return:
    """
    rectPointsSorted = sorted(rectPoints, key=np.sum)
    leftTop = rectPointsSorted[0]
    rightBottom = rectPointsSorted[-1]

    if rectPointsSorted[1][0] > rectPointsSorted[2][0]:
        rightTop = rectPointsSorted[1]
        leftBottom = rectPointsSorted[2]
    else:
        rightTop = rectPointsSorted[2]
        leftBottom = rectPointsSorted[1]

    return tuple(leftTop), tuple(rightTop), tuple(rightBottom), tuple(leftBottom)


def get_linear_fit_k_and_b_vertical(pointsMat):
    """
    垂直方向获取拟合直线,因为垂直方向上,有角度可能很接近90度的情况,如果大于一定的值,就认为是None
    :param pointsMat:
    :return:
    """
    outPut = cv.fitLine(pointsMat, cv.DIST_L2, 0, 0.01, 0.01)
    if outPut[0] != 0:
        k = outPut[1] / outPut[0]
        k = k[0]
        if abs(k) > 1000:
            k = None
            b = np.mean(pointsMat, axis=0)[0]
        else:
            b = outPut[3] - k * outPut[2]
    else:
        k = None
        b = outPut[2]
    return float(k) if k is not None else None, float(b)


def get_ver_light_line(image, verPos, height):
    """
    获取竖直灯管的位置.
    :param image:
    :param verPos:
    :param height:
    :return:
    """
    image_show("imageOriginal", image)
    yStart, yEnd, xStart, xEnd = verPos
    imageUsed = image[yStart:yEnd, xStart:xEnd]
    image_show("ImageUsed", imageUsed)
    T, board = cv.threshold(imageUsed, 180, 255, cv.THRESH_BINARY)
    image_show("ImageThres", board)

    cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    upContourTopMax = 200  # 如果轮廓的第一边是在200以内就认为是上轮廓. 如果是以下,就认为是下轮廓
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterUpDown]
    upContourList = []
    downContourList = []
    upPoint = None
    downPoint = None
    for cnt in largeContours:
        boxPoints = cv.boxPoints(cv.minAreaRect(cnt))
        rectPoints = get_rect_points_clockwise(boxPoints)
        leftTop, rightTop = rectPoints[:2]
        topPointY = get_two_points_middle(leftTop, rightTop)[1]
        if topPointY < upContourTopMax:
            upContourList.append(cnt)
        else:
            downContourList.append(cnt)
    jointLineFlag = False
    if len(upContourList) > 1:
        jointLineFlag = True
        upCenters = [get_center_by_contour(contour) for contour in upContourList]
        for index, center in enumerate(upCenters):
            anotherCenter = upCenters[(index + 1) % len(upCenters)]
            cv.line(board, center, anotherCenter, 255, 20)
        image_show("UpContourJointed", board)

    if len(downContourList) > 1:
        jointLineFlag = True
        upCenters = [get_center_by_contour(contour) for contour in downContourList]
        for index, center in enumerate(upCenters):
            anotherCenter = upCenters[(index + 1) % len(upCenters)]
            cv.line(board, center, anotherCenter, 255, 20)
        image_show("DownContourJointed", board)

    if jointLineFlag:
        cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        cnts = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterUpDown]
        upContourList = []
        downContourList = []
        for cnt in cnts:
            boxPoints = cv.boxPoints(cv.minAreaRect(cnt))
            rectPoints = get_rect_points_clockwise(boxPoints)
            leftTop, rightTop = rectPoints[:2]
            topPointY = get_two_points_middle(leftTop, rightTop)[1]
            if topPointY < upContourTopMax:
                upContourList.append(cnt)
            else:
                downContourList.append(cnt)

    if len(upContourList) >= 1 and len(downContourList) == 0:
        return "error", None, None
        upContour = sorted(upContourList, key=cv.contourArea, reverse=True)[0]
        contourArea = cv.contourArea(upContour)
        if contourArea > 110000:
            return "noBoard", None, None
        else:
            upBox = cv.minAreaRect(upContour)
            upRectPoints = np.int0(cv.boxPoints(upBox))
            upClockwisePoint = get_rect_points_clockwise(upRectPoints)
            upLine = (upClockwisePoint[2], upClockwisePoint[3])
            downLine = (int(upLine[0][0]), int(upLine[0][1] + height)), (int(upLine[1][0]), int(upLine[1][1] + height))
            # 转换到大图上去.
            upLine = (upLine[0][0] + xStart, upLine[0][1] + yStart), (upLine[1][0] + xStart, upLine[1][1] + yStart)
            downLine = (downLine[0][0] + xStart, downLine[0][1] + yStart), (downLine[1][0] + xStart, downLine[1][1] + yStart)

            imageColor = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
            cv.line(imageColor, upLine[0], upLine[1], colorGreen, lineThickness)
            cv.line(imageColor, downLine[0], downLine[1], colorGreen, lineThickness)
            image_show("CalcResByUpContourAndHeight", imageColor)
            upPoint = get_two_points_middle(upLine[0], upLine[1])
            downPoint = get_two_points_middle(downLine[0], downLine[1])

    if len(upContourList) == 0 and len(downContourList) >= 1:
        downContour = sorted(downContourList, key=cv.contourArea, reverse=True)[0]
        contourArea = cv.contourArea(downContour)
        if contourArea > 110000:
            return "noBoard", "竖直灯管没有任何遮挡", None, None, image
        else:
            downBox = cv.minAreaRect(downContour)
            downRectPoints = np.int0(cv.boxPoints(downBox))
            downClockwisePoint = get_rect_points_clockwise(downRectPoints)
            downLine = (downClockwisePoint[0], downClockwisePoint[1])
            upLine = (downLine[0][0], downLine[0][1] - height), (downLine[1][0], downLine[1][1] - height)
            # 转换到大图上去
            upLine = (upLine[0][0] + xStart, upLine[0][1] + yStart), (upLine[1][0] + xStart, upLine[1][1] + yStart)
            downLine = (downLine[0][0] + xStart, downLine[0][1] + yStart), (downLine[1][0] + xStart, downLine[1][1] + yStart)

            imageColor = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
            cv.line(imageColor, upLine[0], upLine[1], colorGreen, lineThickness)
            cv.line(imageColor, downLine[0], downLine[1], colorGreen, lineThickness)
            image_show("CalcResByDownContourAndHeight", imageColor)
            upPoint = get_two_points_middle(upLine[0], upLine[1])
            downPoint = get_two_points_middle(downLine[0], downLine[1])

    if len(upContourList) >= 1 and len(downContourList) >= 1:
        upContour = sorted(upContourList, key=cv.contourArea, reverse=True)[0]
        downContour = sorted(downContourList, key=cv.contourArea, reverse=True)[0]
        upBox = cv.minAreaRect(upContour)
        upRectPoints = np.int0(cv.boxPoints(upBox))
        upClockwisePoint = get_rect_points_clockwise(upRectPoints)
        downBox = cv.minAreaRect(downContour)
        downRectPoints = np.int0(cv.boxPoints(downBox))
        downClockwisePoint = get_rect_points_clockwise(downRectPoints)
        upLine = upClockwisePoint[2], upClockwisePoint[3]
        downLine = downClockwisePoint[0], downClockwisePoint[1]
        # 转换到大图上去
        upLine = (upLine[0][0] + xStart, upLine[0][1] + yStart), (upLine[1][0] + xStart, upLine[1][1] + yStart)
        downLine = (downLine[0][0] + xStart, downLine[0][1] + yStart), (downLine[1][0] + xStart, downLine[1][1] + yStart)
        imageColor = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
        cv.line(imageColor, upLine[0], upLine[1], colorGreen, lineThickness)
        cv.line(imageColor, downLine[0], downLine[1], colorGreen, lineThickness)
        image_show("CalcResByUpContourAndDownContour", imageColor)
        upPoint = get_two_points_middle(upLine[0], upLine[1])
        downPoint = get_two_points_middle(downLine[0], downLine[1])

    if upPoint is not None and downPoint is not None:
        return "ok", upPoint, downPoint
    else:
        return "error", upPoint, downPoint


def get_left_right_line(image, horPos, fillPos, width):
    """
    找左右灯排的位置
    :param image:
    :param horPos:
    :param width:
    :return:
    """
    cv.destroyAllWindows()
    imageCopy = image.copy()
    yStart, yEnd, xStart, xEnd = fillPos
    imageCopy[yStart:yEnd, xStart:xEnd] = 255
    yStart, yEnd, xStart, xEnd = horPos
    imageUsed = imageCopy[yStart:yEnd, xStart:xEnd]
    imageUsed = cv.GaussianBlur(imageUsed, (17, 17), 0)
    imageUsed = cv.morphologyEx(imageUsed, cv.MORPH_CLOSE, (5, 5))
    image_show("ImageUsed", imageUsed)
    imageColor = cv.cvtColor(imageUsed, cv.COLOR_GRAY2BGR)
    T, board = cv.threshold(imageUsed, 200, 255, cv.THRESH_BINARY_INV)
    image_show("ThresBoard", board)
    cnts = cv.findContours(board, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    if len(largeContours) >= 2:
        centerPoints = [get_center_by_contour(contour) for contour in largeContours]
        for index, center in enumerate(centerPoints):
            anotherCenter = centerPoints[(index + 1) % len(centerPoints)]
            cv.line(board, center, anotherCenter, 255, 20)
        image_show("JointCenterLine", board)

        # 先将轮廓延长黑色部分四周延长5个像素
        fillSize = 5
        fillVal = 0
        boardExtend = cv.copyMakeBorder(board, fillSize, fillSize, fillSize, fillSize, fillVal)

        # 连线之后再找一遍轮廓
        cnts = cv.findContours(boardExtend.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        cntFinalFinded = sorted(cnts, key=cv.contourArea, reverse=True)[0]
        rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(cntFinalFinded)))
        # 转换到小图上去
        rectPoints = [(p[0] - fillSize, p[1] - fillSize) for p in rectPoints.tolist()]
        boardColor = imageColor.copy()
        cv.drawContours(boardColor, [np.array(rectPoints)], -1, colorGreen, thickness=3)
        image_show("Contour", boardColor)

        box = cv.minAreaRect(cntFinalFinded)
        (cx, cy), (w, h), angle = box
        if angle > 70:
            angle = 90 - angle
            box = (cx, cy), (h, w), angle

        if angle < -70:
            angle = 90 + angle
            box = (cx, cy), (h, w), angle
        # 找到了左右两个轮廓.然后就是计算这个轮廓的的信息
        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(cv.boxPoints(box))
        print("AngleRect: {}".format(angle))
        leftLineList = []
        rightLineList = []
        # 判断下找的点是不是正确.

        # leftXBig = max(leftTop[0], leftBottom[0])
        # leftXSmall = min(leftTop[0], leftBottom[0])
        # rightXSmall = min(rightTop[0], rightBottom[0])
        # rightXBig = max(rightTop[0], rightBottom[0])
        leftXMin = 2  # 左侧如果找到的位置小于2,就证明找的位置不对
        leftXMax = fillPos[2] - xStart - 10
        leftYMin = leftTop[1]
        leftYMax = leftBottom[1]
        rightXMax = board.shape[1] - 10 - xStart
        rightXmin = fillPos[3] - xStart + 10
        rightYMin = rightTop[1]
        rightYMax = rightBottom[1]
        # leftLineRect = (leftTop,leftBottom)
        # rightLineRect = (rightTop,rightBottom)
        # boardColor = imageColor.copy()
        # cv.line(boardColor,leftLineRect[0],leftLineRect[1],colorGreen,3)
        # cv.line(boardColor,rightLineRect[0],rightLineRect[1],colorGreen,3)
        #
        # image_show("LeftRightLine",boardColor)

        # 先把那个边线找到,左侧的边线的极限位置,右侧的边线的极限位置
        # scharrX = cv.Scharr(board, cv.CV_64F, 1, 0)
        # scharrY = cv.Scharr(board, cv.CV_64F, 0, 1)
        # scharrX = cv.convertScaleAbs(scharrX)
        # scharrY = cv.convertScaleAbs(scharrY)
        # scharrXY = cv.addWeighted(scharrX, 0.2, scharrY, 0.2, 0)
        # scharrXY = cv.convertScaleAbs(scharrXY)
        # image_show("ScharrXY", scharrXY)

        # 先去找线,然后看看是否能找到满足条件的线
        # lines = cv.HoughLinesP(scharrXY.copy(), 1, np.pi / 180, 100, minLineLength, maxLineGap)
        # boardColor = imageColor.copy()
        # leftLineList = []
        # rightLineList = []
        # colorIndex = 0
        # for line in lines:
        #     for x1, y1, x2, y2 in line:
        #         lineLength = distance((x1, y1), (x2, y2))
        #         if abs(x2 - x1) < 50 * cp.mm2pix and abs(y2 - y1) > 8 * cp.mm2pix and leftXMin <= x1 <= leftXMax and leftXMin <= x2 <= leftXMax \
        #                 and leftYMin < y1 < leftYMax and leftYMin < y2 < leftYMax:
        #             leftLineList.append([x1, y1, (x1 + x2) / 2, lineLength])
        #             leftLineList.append([x2, y2, (x1 + x2) / 2, lineLength])
        #             cv.line(boardColor, (x1, y1), (x2, y2), colorList[colorIndex % 3], 3)
        #             colorIndex += 1
        #             continue
        #
        #         if abs(x2 - x1) < 50 * cp.mm2pix and abs(y2 - y1) > 8 * cp.mm2pix and rightXmin <= x1 <= rightXMax and rightXmin <= x2 <= rightXMax \
        #                 and rightYMin < y1 < rightYMax and rightYMin < y2 < rightYMax:
        #             rightLineList.append([x1, y1, (x1 + x2) / 2, lineLength])
        #             rightLineList.append([x2, y2, (x1 + x2) / 2, lineLength])
        #             cv.line(boardColor, (x1, y1), (x2, y2), colorList[colorIndex % 3], 3)
        #             colorIndex += 1
        #             continue
        #
        # image_show("lineFinded", boardColor)

        if len(leftLineList) >= 2:
            leftLineSortedLength = sorted(leftLineList, key=lambda x: x[3], reverse=True)
            leftLine = leftLineSortedLength[:2]
            leftLine = sorted(leftLine, key=lambda x: x[1])  # 按照y轴进行排序
        else:
            leftLine = leftTop, leftBottom

        if len(rightLineList) >= 2:
            rightLineSortedLength = sorted(rightLineList, key=lambda x: x[3], reverse=True)
            rightLine = rightLineSortedLength[:2]
            rightLine = sorted(rightLine, key=lambda x: x[1])
        else:
            rightLine = rightTop, rightBottom

        leftXBig = max(leftLine[0][0], leftLine[1][0])
        leftXSmall = min(leftLine[0][0], leftLine[1][0])
        rightXSmall = min(rightLine[0][0], rightLine[1][0])
        rightXBig = max(rightLine[0][0], rightLine[1][0])

        # 左右两侧的位置都是对的情况.根据右侧的去推测左侧的.先判断是否是有效的
        if leftXMin < leftXBig and leftXSmall < leftXMax and rightXmin < rightXBig and rightXSmall < rightXMax:

            # leftLine = leftTop,leftBottom
            # rightLine = rightTop,rightBottom
            # 定位到大图上去
            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)
        elif leftXBig <= leftXMin and rightXmin < rightXBig and rightXSmall < rightXMax:
            # leftLine = leftTop,leftBottom
            # rightLine = rightTop,rightBottom
            # # 定位到大图上去
            # # 右边的轮廓是对的,但是左边超过了界限
            # rightLine = rightTop,rightBottom
            rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])
            if rightK is None:
                # 如果是垂线,直接延长
                leftLine = (rightLine[0][0] - width, rightLine[0][1]), (rightLine[1][0] - width, rightLine[1][1])
            else:
                # 先求出来垂线的斜率
                verK = -1 / rightK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                leftLine = (rightLine[0][0] - xOffset, rightLine[0][1]), (rightLine[1][0] - xOffset, rightLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)

        elif leftXMin < leftXBig and leftXSmall < leftXMax and rightXSmall > rightXMax:
            # 左边的轮廓是对的,推测右边的轮廓
            # leftLine = leftTop, leftBottom
            leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
            if leftK is None:
                rightLine = (leftLine[0][0] + width, leftLine[0][1]), (leftLine[1][0] + width, leftLine[1][1])
            else:
                verK = -1 / leftK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                rightLine = (leftLine[0][0] + xOffset, leftLine[0][1]), (leftLine[1][0] + xOffset, leftLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)

        else:
            return "error", "轮廓信息不对", None, None


    elif len(largeContours) == 1:  # 只找到一个轮廓,就要判断是左轮廓还是右边的轮廓了.
        cntFinalFinded = largeContours[0]
        # 首先判断是左边的额轮廓,还是右边的轮廓
        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(np.int0(cv.boxPoints(cv.minAreaRect(cntFinalFinded))))
        leftX = max(leftTop[0], leftBottom[0])
        rightX = min(rightTop[0], rightBottom[0])
        if leftX < fillPos[2] and rightX < fillPos[2] + 10:
            # 证明找到的是左边的轮廓,这个是时候腰根据左边的轮廓推测右边的.
            leftLine = leftTop, leftBottom
            leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
            if leftK is None:
                rightLine = (leftLine[0][0] + width, leftLine[0][1]), (leftLine[1][0] + width, leftLine[1][1])
            else:
                verK = -1 / leftK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                rightLine = (leftLine[0][0] + xOffset, leftLine[0][1]), (leftLine[1][0] + xOffset, leftLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)
        else:
            rightLine = rightTop, rightBottom
            rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])
            if rightK is None:
                # 如果是垂线,直接延长
                leftLine = (rightLine[0][0] - width, rightLine[0][1]), (rightLine[1][0] - width, rightLine[1][1])
            else:
                # 先求出来垂线的斜率
                verK = -1 / rightK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                leftLine = (rightLine[0][0] - xOffset, rightLine[0][1]), (rightLine[1][0] - xOffset, rightLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)
    else:
        return "noBoard", "左右没有找到任何轮廓", None, None

    return "ok", "左右两条线查找成功!", leftLineBig, rightLineBig


def get_left_right_line_old(image, horPos, fillPos, width):
    """
    找左右灯排的位置
    :param image:
    :param horPos:
    :param width:
    :return:
    """
    cv.destroyAllWindows()
    imageCopy = image.copy()
    yStart, yEnd, xStart, xEnd = fillPos
    imageCopy[yStart:yEnd, xStart:xEnd] = 255
    yStart, yEnd, xStart, xEnd = horPos
    imageUsed = imageCopy[yStart:yEnd, xStart:xEnd]
    imageUsed = cv.GaussianBlur(imageUsed, (17, 17), 0)
    imageUsed = cv.morphologyEx(imageUsed, cv.MORPH_CLOSE, (5, 5))
    image_show("ImageUsed", imageUsed)
    imageColor = cv.cvtColor(imageUsed, cv.COLOR_GRAY2BGR)
    T, board = cv.threshold(imageUsed, 200, 255, cv.THRESH_BINARY_INV)
    image_show("ThresBoard", board)
    cnts = cv.findContours(board, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    if len(largeContours) >= 2:
        centerPoints = [get_center_by_contour(contour) for contour in largeContours]
        for index, center in enumerate(centerPoints):
            anotherCenter = centerPoints[(index + 1) % len(centerPoints)]
            cv.line(board, center, anotherCenter, 255, 20)
        image_show("JointCenterLine", board)

        # 连线之后再找一遍轮廓
        cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        cntFinalFinded = sorted(cnts, key=cv.contourArea, reverse=True)[0]

        # 找到了左右两个轮廓.然后就是计算这个轮廓的的信息
        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(np.int0(cv.boxPoints(cv.minAreaRect(cntFinalFinded))))
        # 判断下找的点是不是正确.
        leftXMin = 2  # 左侧如果找到的位置小于2,就证明找的位置不对
        leftXMax = fillPos[2] - xStart - 10
        rightXMax = board.shape[1] - 10 - xStart
        rightXmin = fillPos[3] - xStart + 10
        leftXBig = max(leftTop[0], leftBottom[0])
        leftXSmall = min(leftTop[0], leftBottom[0])
        rightXSmall = min(rightTop[0], rightBottom[0])
        rightXBig = max(rightTop[0], rightBottom[0])

        # 先把那个边线找到,左侧的边线的极限位置,右侧的边线的极限位置
        scharrX = cv.Scharr(board, cv.CV_64F, 1, 0)
        scharrY = cv.Scharr(board, cv.CV_64F, 0, 1)
        scharrX = cv.convertScaleAbs(scharrX)
        scharrY = cv.convertScaleAbs(scharrY)
        scharrXY = cv.addWeighted(scharrX, 0.2, scharrY, 0.2, 0)
        scharrXY = cv.convertScaleAbs(scharrXY)
        image_show("ScharrXY", scharrXY)

        # 先去找线,然后看看是否能找到满足条件的线
        lines = cv.HoughLinesP(scharrXY.copy(), 1, np.pi / 180, 100, minLineLength, maxLineGap)
        boardColor = imageColor.copy()
        leftLineList = []
        rightLineList = []
        colorIndex = 0
        for line in lines:
            for x1, y1, x2, y2 in line:
                lineLength = distance((x1, y1), (x2, y2))
                if abs(x2 - x1) < 20 * cp.mm2pix and abs(y2 - y1) > 8 * cp.mm2pix and leftXMin <= x1 <= leftXMax and leftXMin <= x2 <= leftXMax:
                    leftLineList.append([x1, y1, (x1 + x2) / 2, lineLength])
                    leftLineList.append([x2, y2, (x1 + x2) / 2, lineLength])
                    cv.line(boardColor, (x1, y1), (x2, y2), colorList[colorIndex % 3], 3)
                    colorIndex += 1
                    continue

                if abs(x2 - x1) < 20 * cp.mm2pix and abs(y2 - y1) > 8 * cp.mm2pix and rightXmin <= x1 <= rightXMax and rightXmin <= x2 <= rightXMax:
                    rightLineList.append([x1, y1, (x1 + x2) / 2, lineLength])
                    rightLineList.append([x2, y2, (x1 + x2) / 2, lineLength])
                    cv.line(boardColor, (x1, y1), (x2, y2), colorList[colorIndex % 3], 3)
                    colorIndex += 1
                    continue

        image_show("lineFinded", boardColor)

        if len(leftLineList) >= 2:
            leftLineSortedLength = sorted(leftLineList, key=lambda x: x[3], reverse=True)
            leftLine = leftLineSortedLength[:2]
            leftLine = sorted(leftLine, key=lambda x: x[1])  # 按照y轴进行排序
        else:
            leftLine = leftTop, leftBottom

        if len(rightLineList) >= 2:
            rightLineSortedLength = sorted(rightLineList, key=lambda x: x[3], reverse=True)
            rightLine = rightLineSortedLength[:2]
            rightLine = sorted(rightLine, key=lambda x: x[1])
        else:
            rightLine = rightTop, rightBottom

        leftXBig = max(leftLine[0][0], leftLine[1][0])
        leftXSmall = min(leftLine[0][0], leftLine[1][0])
        rightXSmall = min(rightLine[0][0], rightLine[1][0])
        rightXBig = max(rightLine[0][0], rightLine[1][0])

        # 左右两侧的位置都是对的情况.根据右侧的去推测左侧的.先判断是否是有效的
        if leftXMin < leftXBig and leftXSmall < leftXMax and rightXmin < rightXBig and rightXSmall < rightXMax:

            # leftLine = leftTop,leftBottom
            # rightLine = rightTop,rightBottom
            # 定位到大图上去
            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)
        elif leftXBig <= leftXMin and rightXmin < rightXBig and rightXSmall < rightXMax:
            # leftLine = leftTop,leftBottom
            # rightLine = rightTop,rightBottom
            # # 定位到大图上去
            # # 右边的轮廓是对的,但是左边超过了界限
            # rightLine = rightTop,rightBottom
            rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])
            if rightK is None:
                # 如果是垂线,直接延长
                leftLine = (rightLine[0][0] - width, rightLine[0][1]), (rightLine[1][0] - width, rightLine[1][1])
            else:
                # 先求出来垂线的斜率
                verK = -1 / rightK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                leftLine = (rightLine[0][0] - xOffset, rightLine[0][1]), (rightLine[1][0] - xOffset, rightLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)

        elif leftXMin < leftXBig and leftXSmall < leftXMax and rightXSmall > rightXMax:
            # 左边的轮廓是对的,推测右边的轮廓
            # leftLine = leftTop, leftBottom
            leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
            if leftK is None:
                rightLine = (leftLine[0][0] + width, leftLine[0][1]), (leftLine[1][0] + width, leftLine[1][1])
            else:
                verK = -1 / leftK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                rightLine = (leftLine[0][0] + xOffset, leftLine[0][1]), (leftLine[1][0] + xOffset, leftLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)

        else:
            return "error", "轮廓信息不对", None, None


    elif len(largeContours) == 1:  # 只找到一个轮廓,就要判断是左轮廓还是右边的轮廓了.
        cntFinalFinded = largeContours[0]
        # 首先判断是左边的额轮廓,还是右边的轮廓
        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(np.int0(cv.boxPoints(cv.minAreaRect(cntFinalFinded))))
        leftX = max(leftTop[0], leftBottom[0])
        rightX = min(rightTop[0], rightBottom[0])
        if leftX < fillPos[2] and rightX < fillPos[2] + 10:
            # 证明找到的是左边的轮廓,这个是时候腰根据左边的轮廓推测右边的.
            leftLine = leftTop, leftBottom
            leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
            if leftK is None:
                rightLine = (leftLine[0][0] + width, leftLine[0][1]), (leftLine[1][0] + width, leftLine[1][1])
            else:
                verK = -1 / leftK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                rightLine = (leftLine[0][0] + xOffset, leftLine[0][1]), (leftLine[1][0] + xOffset, leftLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)
        else:
            rightLine = rightTop, rightBottom
            rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])
            if rightK is None:
                # 如果是垂线,直接延长
                leftLine = (rightLine[0][0] - width, rightLine[0][1]), (rightLine[1][0] - width, rightLine[1][1])
            else:
                # 先求出来垂线的斜率
                verK = -1 / rightK
                xOffset = np.cos(trans_k_to_rad(verK)) * width
                leftLine = (rightLine[0][0] - xOffset, rightLine[0][1]), (rightLine[1][0] - xOffset, rightLine[1][1])

            leftLineBig = (leftLine[0][0] + xStart, leftLine[0][1] + yStart), (leftLine[1][0] + xStart, leftLine[1][1] + yStart)
            rightLineBig = (rightLine[0][0] + xStart, rightLine[0][1] + yStart), (rightLine[1][0] + xStart, rightLine[1][1] + yStart)
    else:
        return "noBoard", "左右没有找到任何轮廓", None, None

    return "ok", "左右两条线查找成功!", leftLineBig, rightLineBig


def find_left_line(leftImage):
    """
    先找左边线,如果左边线找不到,就报警,证明板子倾斜度太大
    :param lefImage:
    :return:
    """
    cv.destroyAllWindows()
    image_show("LeftImage", leftImage)
    imageColor = cv.cvtColor(leftImage, cv.COLOR_GRAY2BGR)
    leftImage = cv.GaussianBlur(leftImage, (0, 0), 5)
    image_show("LeftImageBlurred", leftImage)
    T, board = cv.threshold(leftImage, 210, 255, cv.THRESH_BINARY_INV)
    image_show("leftBoard", board)
    board = cv.morphologyEx(board, cv.MORPH_CLOSE, (5, 5), iterations=3)
    image_show("leftBoard", board)
    cnts = cv.findContours(board, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    if len(largeContours) >= 1:
        largeContoursSorted = sorted(largeContours, key=cv.contourArea, reverse=True)
        contourFinded = largeContoursSorted[0]
        rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(contourFinded)))
        boardColor = imageColor.copy()
        cv.drawContours(boardColor, [np.array(rectPoints)], -1, colorGreen, thickness=3)
        image_show("LeftContour", boardColor)
        box = cv.minAreaRect(contourFinded)
        (cx, cy), (w, h), angle = box
        if angle > 70:
            angle = 90 - angle
            box = (cx, cy), (h, w), angle
        if angle < -70:
            angle = 90 + angle
            box = (cx, cy), (h, w), angle

        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(cv.boxPoints(box))

        # 这里找到以后,再使用哪个scharrXY再找一次,先将图像抠出来
        sliceX = int(min(leftTop[0], leftBottom[0]))
        sliceX = sliceX - 5 if sliceX > 5 else sliceX  # 如果大于,就再往左边截五个像素
        yStart = int(min(leftTop[1], rightTop[1]))
        yEnd = int(max(leftBottom[1], rightBottom[1]))

        sliceX = sliceX if sliceX >= 0 else 0
        yStart = yStart if yStart >= 0 else 0
        yEnd = yEnd if yEnd <= board.shape[0] else board.shape[0]

        imageUsed = board[yStart:yEnd, sliceX:]
        image_show("ImageUsedFindedLeftLine", imageUsed)
        cv.destroyAllWindows()
        scharrX = cv.Scharr(imageUsed, cv.CV_64F, 1, 0)
        scharrY = cv.Scharr(imageUsed, cv.CV_64F, 0, 1)
        scharrX = cv.convertScaleAbs(scharrX)
        scharrY = cv.convertScaleAbs(scharrY)
        scharrXY = cv.addWeighted(scharrX, 0.3, scharrY, 0.2, 0)
        findLineBoard = scharrXY.copy()
        image_show("ScharrXYLeftLine", findLineBoard)
        lines = cv.HoughLinesP(findLineBoard, 1, np.pi / 180, 100, minLineLength, maxLineGap)
        boardShow = imageColor.copy()
        linesList = []
        linesListTop = []  # 上边的线条
        linesListBottom = []  # 下边的线条
        # colorIndex = 0
        # xOffset = sliceX
        # yOffset = yStart
        # if lines is not None:
        #     for line in lines:
        #         for x1,y1,x2,y2 in line:
        #             lineLength = get_two_point_distance((x1,y1),(x2,y2))
        #             xMin = 0
        #             xMax = boardShow.shape[1] * 0.8
        #             if abs(x2 - x1) < 10 * cp.mm2pix and abs(y2 - y1) > 10 * cp.mm2pix and xMin < x1 < xMax and xMin < x2 < xMax:
        #                 x1 += xOffset
        #                 x2 += xOffset
        #                 y1 += yOffset
        #                 y2 += yOffset
        #                 lineTemp = [(x1,y1),(x2,y2)]
        #                 linesList.append([lineTemp,(x1 + x2)/ 2, lineLength])
        #                 cv.line(boardShow,(x1,y1),(x2,y2),colorList[colorIndex % 3],2)
        #                 colorIndex += 1
        #
        #             topLineYMin = 0
        #             topLineYMax = imageUsed.shape[0] * 0.3
        #             if abs(x2 - x1) > 10 * cp.mm2pix and abs(y2 - y1) < 10 * cp.mm2pix and xMin < x1 < xMax and xMin < x2 < xMax  and \
        #                     topLineYMin < y1 < topLineYMax and topLineYMin < y2 < topLineYMax:
        #                 x1 += xOffset
        #                 x2 += xOffset
        #                 y1 += yOffset
        #                 y2 += yOffset
        #                 lineTemp = [(x1,y1),(x2,y2)]
        #                 linesListTop.append([lineTemp,(y1 + y2) / 2,lineLength])
        #                 cv.line(boardShow,(x1,y1),(x2,y2),colorList[colorIndex%3],2)
        #                 colorIndex += 1
        #
        #
        #
        #             bottomLineYMin = imageUsed.shape[0] * 0.8
        #             bottomLineYMax = imageUsed.shape[0]
        #             if abs(x2 - x1) > 10 * cp.mm2pix and abs(y2 - y1) < 10 * cp.mm2pix and xMin < x1 < xMax and xMin < x2 < xMax  and \
        #                     bottomLineYMin < y1 < bottomLineYMax and bottomLineYMin < y2 < bottomLineYMax:
        #                 x1 += xOffset
        #                 x2 += xOffset
        #                 y1 += yOffset
        #                 y2 += yOffset
        #                 lineTemp = [(x1,y1),(x2,y2)]
        #                 linesListBottom.append([lineTemp,(y1 + y2) / 2,lineLength])
        #                 cv.line(boardShow,(x1,y1),(x2,y2),colorList[colorIndex%3],2)
        #                 colorIndex += 1

        if len(linesListTop) >= 1:
            image_show("leftLineFinded", boardShow)
            linesSortedLength = sorted(linesListTop, key=lambda x: x[2], reverse=True)
            lineFinalFinded = linesSortedLength[0][0]
            topK, topB = get_line_k_and_b(lineFinalFinded[0], lineFinalFinded[1])
        else:
            topK, topB = get_line_k_and_b(leftTop, rightTop)

        if len(linesListBottom) >= 1:
            image_show("leftLineFinded", boardShow)
            linesSortedLength = sorted(linesListBottom, key=lambda x: x[2], reverse=True)
            lineFinalFinded = linesSortedLength[0][0]
            bottomK, bottomB = get_line_k_and_b(lineFinalFinded[0], lineFinalFinded[1])
        else:
            bottomK, bottomB = get_line_k_and_b(leftBottom, rightBottom)

        if len(linesList) >= 1:
            image_show("leftLineFinded", boardShow)
            linesListSortedX = sorted(linesList, key=lambda x: x[2], reverse=True)
            midIndex = int(len(linesListSortedX) / 2)
            lineFinalFinded = linesListSortedX[0][0]
            leftK, leftB = get_line_k_and_b(lineFinalFinded[0], lineFinalFinded[1])
            # topK,topB = get_line_k_and_b(leftTop,rightTop)
            # bottomK,bottomB = get_line_k_and_b(leftBottom,rightBottom)
            leftTop = get_two_lines_intersection(leftK, leftB, topK, topB)
            leftBottom = get_two_lines_intersection(leftK, leftB, bottomK, bottomB)
        else:
            leftK = get_line_k_and_b(leftTop, leftBottom)[0]

        # 然后重新计算四个点,现在确定的点是leftTop,leftBottom. topK,bottomK
        x = leftImage.shape[1]
        y1 = int(topK * x + topB)
        y2 = int(bottomK * x + bottomB)
        rightTop = (x, y1)
        rightBottom = (x, y2)

        boardShow = imageColor.copy()
        cv.line(boardShow, leftTop, leftBottom, colorGreen, 3)
        cv.line(boardShow, leftTop, rightTop, colorGreen, 3)
        cv.line(boardShow, leftBottom, rightBottom, colorGreen, 3)
        image_show("LeftLineFinalFinded", boardShow)
        angle = get_angle_by_k(leftK)
        if angle < 0:
            angle = angle + 90
        else:
            angle = angle - 90
        print("lefAngle: {}".format(angle))
        leftTopMid = get_two_points_middle(leftTop, rightTop)
        leftBottomMid = get_two_points_middle(leftBottom, rightBottom)
        leftLine = [leftTop, leftBottom]
        leftMid = get_two_points_middle(leftTop, leftBottom)
        if leftMid[0] == 0 or leftTopMid[1] == 0 or leftBottomMid[1] == leftImage.shape[0]:
            return "error", "左边识别的轮廓位置超出边界,请手动移动下板子,然后重新检测!", None, None, None, None
        else:
            leftLine = [[int(p[0]), int(p[1])] for p in leftLine]
            leftTopMid = [int(leftTopMid[0]), int(leftTopMid[1])]
            leftBottomMid = [int(leftBottomMid[0]), int(leftBottomMid[1])]
            return "ok", "左边识别成功", leftLine, leftTopMid, leftBottomMid, angle
    else:
        return "error", "识别失败,左边没有找到任何轮廓", None, None, None, None


def get_two_point_distance(p1, p2):
    """
    获取两点之间的距离,保留小数点后两位
    :param p1: 点1
    :param p2: 点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    ds = np.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2)
    return round(ds, 2)


def find_right_line(rightImage, xOffset):
    """
    先找左边线,如果左边线找不到,就报警,证明板子倾斜度太大
    :param lefImage:
    :return:
    """
    cv.destroyAllWindows()
    image_show("RightImage", rightImage)
    imageColor = cv.cvtColor(rightImage, cv.COLOR_GRAY2BGR)
    rightImage = cv.GaussianBlur(rightImage, (0, 0), 5)
    image_show("RightImageOpened", rightImage)
    T, board = cv.threshold(rightImage, 200, 255, cv.THRESH_BINARY_INV)
    board = cv.morphologyEx(board, cv.MORPH_OPEN, (5, 5), iterations=3)
    image_show("rightBoard", board)
    cnts = cv.findContours(board, cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    if len(largeContours) >= 1:
        largeContoursSorted = sorted(largeContours, key=cv.contourArea, reverse=True)
        contourFinded = largeContoursSorted[0]
        rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(contourFinded)))
        boardColor = imageColor.copy()
        cv.drawContours(boardColor, [np.array(rectPoints)], -1, colorGreen, thickness=3)
        image_show("RightContour", boardColor)
        box = cv.minAreaRect(contourFinded)
        (cx, cy), (w, h), angle = box
        if angle > 70:
            angle = 90 - angle
            box = (cx, cy), (h, w), angle
        if angle < -70:
            angle = 90 + angle
            box = (cx, cy), (h, w), angle

        leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(cv.boxPoints(box))

        # sliceX = int(max(rightTop[0],rightBottom[0]))
        # sliceX = sliceX + 5 if sliceX + 5 < rightImage.shape[1] else sliceX
        # yStart = int(min(leftTop[1],rightTop[1]))
        # yEnd = int(max(leftBottom[1],rightBottom[1]))
        # yStart = yStart if yStart >= 0 else 0
        # yEnd = yEnd if yEnd <= board.shape[0] else board.shape[0]
        # imageUsed = board[yStart:yEnd,:sliceX]
        # image_show("ImageUsedFindRightLine",imageUsed)
        # cv.destroyAllWindows()
        #
        # scharrX = cv.Scharr(imageUsed, cv.CV_64F, 1, 0)
        # scharrY = cv.Scharr(imageUsed, cv.CV_64F, 0, 1)
        # scharrX = cv.convertScaleAbs(scharrX)
        # scharrY = cv.convertScaleAbs(scharrY)
        # scharrXY = cv.addWeighted(scharrX, 0.5, scharrY, 0.2, 0)
        # findLineBoard = scharrXY.copy()
        # image_show("ScharrXYLeftLine", findLineBoard)
        # lines = cv.HoughLinesP(findLineBoard, 1, np.pi / 180, 100, minLineLength, maxLineGap)
        boardShow = imageColor.copy()
        linesList = []
        linesListTop = []
        linesListBottom = []
        # colorIndex = 0
        # yOffset = yStart
        # if lines is not None:
        #     for line in lines:
        #         for x1, y1, x2, y2 in line:
        #             lineLength = get_two_point_distance((x1, y1), (x2, y2))
        #             xMin = boardShow.shape[1] * 0.1
        #             xMax = boardShow.shape[1]
        #             if abs(x2 - x1) < 10 * cp.mm2pix and abs(y2 - y1) > 5 * cp.mm2pix and xMin < x1 < xMax and xMin < x2 < xMax:
        #                 y1 += yOffset
        #                 y2 += yOffset
        #                 lineTemp = [(x1, y1), (x2, y2)]
        #                 linesList.append([lineTemp, (x1 + x2) / 2, lineLength])
        #                 cv.line(boardShow, (x1, y1), (x2, y2), colorList[colorIndex % 3], 2)
        #                 colorIndex += 1
        #
        #             topLineYMin = 0
        #             topLineYMax = imageUsed.shape[0] * 0.3
        #             if abs(x2 - x1) > 10 * cp.mm2pix and abs(y2 - y1) < 10 * cp.mm2pix and xMin < x1 < xMax and xMin < x2 < xMax and \
        #                     topLineYMin < y1 < topLineYMax and topLineYMin < y2 < topLineYMax:
        #                 y1 += yOffset
        #                 y2 += yOffset
        #                 lineTemp = [(x1, y1), (x2, y2)]
        #                 linesListTop.append([lineTemp, (y1 + y2) / 2, lineLength])
        #                 cv.line(boardShow, (x1, y1), (x2, y2), colorList[colorIndex % 3], 2)
        #                 colorIndex += 1
        #
        #             bottomLineYMin = imageUsed.shape[0] * 0.8
        #             bottomLineYMax = imageUsed.shape[0]
        #             if abs(x2 - x1) > 10 * cp.mm2pix and abs(y2 - y1) < 10 * cp.mm2pix and xMin < x1 < xMax and xMin < x2 < xMax and \
        #                     bottomLineYMin < y1 < bottomLineYMax and bottomLineYMin < y2 < bottomLineYMax:
        #                 y1 += yOffset
        #                 y2 += yOffset
        #                 lineTemp = [(x1, y1), (x2, y2)]
        #                 linesListBottom.append([lineTemp, (y1 + y2) / 2, lineLength])
        #                 cv.line(boardShow, (x1, y1), (x2, y2), colorList[colorIndex % 3], 2)
        #                 colorIndex += 1
        if len(linesListTop) >= 1:
            image_show("rightLineFinded", boardShow)
            linesSortedLength = sorted(linesListTop, key=lambda x: x[2], reverse=True)
            lineFinalFinded = linesSortedLength[0][0]
            topK, topB = get_line_k_and_b(lineFinalFinded[0], lineFinalFinded[1])
        else:
            topK, topB = get_line_k_and_b(leftTop, rightTop)

        if len(linesListBottom) >= 1:
            image_show("rightLineFinded", boardShow)
            linesSortedLength = sorted(linesListBottom, key=lambda x: x[2], reverse=True)
            lineFinalFinded = linesSortedLength[0][0]
            bottomK, bottomB = get_line_k_and_b(lineFinalFinded[0], lineFinalFinded[1])
        else:
            bottomK, bottomB = get_line_k_and_b(leftBottom, rightBottom)

        if len(linesList) >= 1:
            image_show("rightLineFinded", boardShow)
            linesListSortedX = sorted(linesList, key=lambda x: x[2], reverse=True)
            midIndex = int(len(linesListSortedX) / 2)
            lineFinalFinded = linesListSortedX[0][0]
            rightK, rightB = get_line_k_and_b(lineFinalFinded[0], lineFinalFinded[1])
            # topK, topB = get_line_k_and_b(leftTop, rightTop)
            # bottomK, bottomB = get_line_k_and_b(leftBottom, rightBottom)
            rightTop = get_two_lines_intersection(rightK, rightB, topK, topB)
            rightBottom = get_two_lines_intersection(rightK, rightB, bottomK, bottomB)
        else:
            rightK = get_line_k_and_b(rightTop, rightBottom)[0]

        # 现在确定的点是rightTop,rightBottom,还设有topK,topB,bottomK,bottomB
        x = 0
        y1 = int(topK * x + topB)
        y2 = int(bottomK * x + bottomB)
        leftTop = (x, y1)
        leftBottom = (x, y2)

        boardShow = imageColor.copy()
        cv.line(boardShow, rightTop, rightBottom, colorGreen, 3)
        cv.line(boardShow, leftTop, rightTop, colorGreen, 3)
        cv.line(boardShow, leftBottom, rightBottom, colorGreen, 3)
        image_show("rightLineFinalFinded", boardShow)

        angle = get_angle_by_k(rightK)
        if angle < 0:
            angle = angle + 90
        else:
            angle = angle - 90
        print("RightAngle: {}".format(angle))
        rightTopMid = get_two_points_middle(leftTop, rightTop)
        rightBottomMid = get_two_points_middle(leftBottom, rightBottom)
        rightLine = [rightTop, rightBottom]
        rigthMid = get_two_points_middle(rightTop, rightBottom)
        if rigthMid[0] == rightImage.shape[1] or rightTopMid[1] == 0 or rightBottomMid[1] == rightImage.shape[0]:
            return "error", "右边识别的轮廓位置超出边界,请手动移动下板子,然后重新检测!", None, None, None, None
        else:
            # 将左边转换到大图上去,就是y方向的偏移问题
            rightLine = [[int(rightLine[0][0] + xOffset), int(rightLine[0][1])], [int(rightLine[1][0] + xOffset), int(rightLine[1][1])]]
            rightTopMid = int(rightTopMid[0] + xOffset), int(rightTopMid[1])
            rightBottomMid = int(rightBottomMid[0] + xOffset), int(rightBottomMid[1])
            return "ok", "右边识别成功", rightLine, rightTopMid, rightBottomMid, angle
    else:
        return "error", "识别失败,右边没有找到任何轮廓", None, None, None, None


def find_total_board_info(image):
    # Y轴方向上的截取位置
    imageSliceY = image.copy()
    image_show("ImageOriginal", imageSliceY)
    imageColor = cv.cvtColor(imageSliceY, cv.COLOR_GRAY2BGR)
    # 先把图像分成左右两个部分
    # 中间的两个型材位置
    xPos1 = 1203
    xPos2 = 1728
    leftImage = imageSliceY[:, :xPos1]
    rightImage = imageSliceY[:, xPos2:]

    leftRes, leftInfo, leftLine, leftTopMid, leftBottomMid, leftAngle = find_left_line(leftImage)
    if leftRes != "ok":
        return "error", leftInfo, None, None, imageColor
    rightRes, rightInfo, rightLine, rightTopMid, rightBottomMid, rightAngle = find_right_line(rightImage, xPos2)
    if rightRes != "ok":
        return "error", rightInfo, None, None, imageColor

    # 根据左右图的坐标,计算大图的坐标:
    angleMean = round((leftAngle + rightAngle) / 2, 3)
    leftLine = leftLine
    rightLine = rightLine

    # topLine要做延长的
    topK, topB = get_line_k_and_b(leftTopMid, rightTopMid)
    bottomK, bottomB = get_line_k_and_b(leftBottomMid, rightBottomMid)
    leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
    rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])

    leftTopIntersection = get_two_lines_intersection(leftK, leftB, topK, topB)
    rightTopIntersection = get_two_lines_intersection(rightK, rightB, topK, topB)
    leftBottomIntersection = get_two_lines_intersection(leftK, leftB, bottomK, bottomB)
    rightBottomIntersection = get_two_lines_intersection(rightK, rightB, bottomK, bottomB)

    topLine = [leftTopIntersection, rightTopIntersection]
    bottomLine = [leftBottomIntersection, rightBottomIntersection]

    cv.line(imageColor, tuple(leftLine[0]), tuple(leftLine[1]), colorGreen, thickness=3)
    cv.line(imageColor, tuple(rightLine[0]), tuple(rightLine[1]), colorGreen, thickness=3)
    cv.line(imageColor, tuple(topLine[0]), tuple(topLine[1]), colorGreen, thickness=5)
    cv.line(imageColor, tuple(bottomLine[0]), tuple(bottomLine[1]), colorGreen, thickness=3)
    image_show("TotalContourFinded", imageColor)

    # 固定边的偏移值
    yOffset = int(cp.catchPosOffset4pin * cp.mm2pix)
    fixLineMid = get_two_points_middle(leftBottomIntersection, rightBottomIntersection)
    # 固定边是topLine,按照这个line做垂线
    if topK is None:
        verK = 0
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] - abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = verB
    elif topK == 0:
        catchX = fixLineMid[0]
        catchY = fixLineMid[1] - yOffset
    else:
        verK = -1 / topK
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] - abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = (catchY - verB) / verK

    catchPos = (int(catchX), int(catchY))
    print(catchX, catchY)
    interSection5 = np.int0(fixLineMid)

    cv.line(imageColor, tuple(interSection5), catchPos, colorRed, lineThickness)
    cv.circle(imageColor, catchPos, 5, colorRed, -1)
    cv.putText(imageColor, "angle:{:.2f}".format(angleMean), (catchPos[0] - 50, catchPos[1] - 200),
               cv.FONT_HERSHEY_SIMPLEX, 2, colorRed, 4)
    cv.putText(imageColor, "catchPos:{}".format(catchPos), (catchPos[0] - 50, catchPos[1] - 30),
               cv.FONT_HERSHEY_SIMPLEX, 2, colorRed, 4)
    print("Angle: {:.2f},catchPos:{}".format(angleMean, catchPos))

    image_show("TotalBoard", imageColor)

    catchPosRobot = get_module_catch_pos(catchPos)
    if ROBOT_LIMIT_X_MIN < catchPosRobot[0] < ROBOT_LIMIT_X_MAX and ROBOT_LIMIT_Y_MIN < catchPosRobot[1] < ROBOT_LIMIT_Y_Max and \
            abs(angleMean) < cp.errorAngleThres:
        return "ok", "识别成功!", catchPosRobot, angleMean, imageColor
    elif abs(angleMean) >= cp.errorAngleThres:
        return "error", "识别的角度太大,识别角度:{:.2f},阈值:{:.2f}".format(angleMean, cp.errorAngleThres), catchPosRobot, angleMean, imageColor
    else:
        info = "识别的坐标超过极限位置,识别的抓取坐标X:{:.2f},ok范围:({},{}),坐标Y:{:.2f},ok范围:({},{})".format(
            catchPosRobot[0], ROBOT_LIMIT_X_MIN, ROBOT_LIMIT_X_MAX, catchPosRobot[1], ROBOT_LIMIT_Y_MIN, ROBOT_LIMIT_Y_Max)
        return "error", info, catchPosRobot, angleMean, imageColor

    # # 获取抓取位置偏移
    # yOffset = int(cp.catchPosOffset4pin * cp.mm2pix)
    #
    # fixK, fixB = get_line_k_and_b(interSection3, interSection4)
    # # 做右边线的垂线,根据那个补偿值
    # fixLineMid = get_two_points_middle(interSection3, interSection4)
    # print(fixK, fixB)
    # print(fixLineMid)
    # if fixK == 0:
    #     catchX = fixLineMid[0]
    #     catchY = fixLineMid[1] - yOffset
    # else:
    #     verK = -1 / fixK
    #     verB = fixLineMid[1] - verK * fixLineMid[0]
    #     catchY = fixLineMid[1] - abs(yOffset * np.sin(trans_k_to_rad(verK)))
    #     catchX = (catchY - verB) / verK
    #
    # # if meanLineK is None:
    # #     fixMiddle = get_two_points_middle(interSection3, interSection4)
    # #     catchX = fixMiddle[0]
    # #     catchY = fixMiddle[1] - yOffset
    # # else:
    # #     fixMiddle = get_two_points_middle(interSection3, interSection4)
    # #     # catchX = fixMiddle[0] - abs(yOffset * np.cos(trans_k_to_rad(meanLineK)))
    # #     # catchY = meanLineK * catchX + meanLineB
    # #     catchY = fixMiddle[1] - abs(yOffset * np.sin(trans_k_to_rad(meanLineK)))
    # #     catchX = (catchY - meanLineB) / meanLineK
    # # if angle < 0:
    # #     angle = round(angle + 90, 2)
    # # else:
    # #     angle = round(angle - 90, 2)
    # print(catchX, catchY)
    #
    # catchPos = (int(catchX), int(catchY))
    # # 和固定边的交点
    # interSection5 = np.int0(fixLineMid)
    # # (x,y)坐标颠倒
    # # interSection5 = (interSection5[1],interSection5[0])
    #
    # cv.line(imageColorContour, tuple(interSection5), catchPos, colorRed, lineThickness)
    # cv.circle(imageColorContour, catchPos, 5, colorRed, -1)
    # cv.putText(imageColorContour, "angle:{:.2f}".format(angle), (catchPos[0] - 100, catchPos[1] - 200),
    #            cv.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
    # cv.putText(imageColorContour, "catchPos:{}".format(catchPos), (catchPos[0] - 100, catchPos[1] - 30), cv.FONT_HERSHEY_SIMPLEX,
    #            2, (255, 0, 0), 4)
    # print("Angle: {:.2f},catchPos:{}".format(angle, catchPos))
    # image_show("TotalBoard", imageColorContour)
    #
    # # 根据图像抓取位置,获取实际的坐标偏移
    # catchPosRobot = get_module_catch_pos(catchPos)
    # if ROBOT_LIMIT_X_MIN < catchPosRobot[0] < ROBOT_LIMIT_X_MAX and ROBOT_LIMIT_Y_MIN < catchPosRobot[1] < ROBOT_LIMIT_Y_Max and \
    #         abs(angle) < cp.errorAngleThres:
    #     return "ok", "识别成功!", catchPosRobot, angle, imageColorContour
    # elif abs(angle) >= cp.errorAngleThres:
    #     return "error","识别的角度太大,识别角度:{:.2f},阈值:{:.2f}".format(angle,cp.errorAngleThres),catchPosRobot,angle,imageColorContour
    # else:
    #     info = "识别的坐标超过极限位置,识别的抓取坐标X:{:.2f},ok范围:({},{}),坐标Y:{:.2f},ok范围:({},{})".format(
    #         catchPosRobot[0],ROBOT_LIMIT_X_MIN,ROBOT_LIMIT_X_MAX,catchPosRobot[1],ROBOT_LIMIT_Y_MIN,ROBOT_LIMIT_Y_Max)
    #     return "error",info , catchPosRobot, angle, imageColorContour


def find_total_board_info_6pin(image, width, height):
    cv.destroyAllWindows()
    imageSliceY = image.copy()
    image_show("ImageOriginal", imageSliceY)
    imageColor = cv.cvtColor(imageSliceY, cv.COLOR_GRAY2BGR)
    # 先把图像分成左右两个部分
    # 中间的两个型材位置
    xPos1 = 1203
    xPos2 = 1728
    leftImage = imageSliceY[:, :xPos1]
    rightImage = imageSliceY[:, xPos2:]

    leftRes, leftInfo, leftLine, leftTopMid, leftBottomMid, leftAngle = find_left_line(leftImage)
    if leftRes != "ok":
        return "error", leftInfo, None, None, imageColor
    rightRes, rightInfo, rightLine, rightTopMid, rightBottomMid, rightAngle = find_right_line(rightImage, xPos2)
    if rightRes != "ok":
        return "error", rightInfo, None, None, imageColor

    # 根据左右图的坐标,计算大图的坐标:
    angleMean = round((leftAngle + rightAngle) / 2, 3)
    leftLine = leftLine
    rightLine = rightLine

    # topLine要做延长的
    topK, topB = get_line_k_and_b(leftTopMid, rightTopMid)
    bottomK, bottomB = get_line_k_and_b(leftBottomMid, rightBottomMid)
    leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
    rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])

    leftTopIntersection = get_two_lines_intersection(leftK, leftB, topK, topB)
    rightTopIntersection = get_two_lines_intersection(rightK, rightB, topK, topB)
    leftBottomIntersection = get_two_lines_intersection(leftK, leftB, bottomK, bottomB)
    rightBottomIntersection = get_two_lines_intersection(rightK, rightB, bottomK, bottomB)

    topLine = [leftTopIntersection, rightTopIntersection]
    bottomLine = [leftBottomIntersection, rightBottomIntersection]

    cv.line(imageColor, tuple(leftLine[0]), tuple(leftLine[1]), colorGreen, thickness=3)
    cv.line(imageColor, tuple(rightLine[0]), tuple(rightLine[1]), colorGreen, thickness=3)
    cv.line(imageColor, tuple(topLine[0]), tuple(topLine[1]), colorGreen, thickness=5)
    cv.line(imageColor, tuple(bottomLine[0]), tuple(bottomLine[1]), colorGreen, thickness=3)
    image_show("TotalContourFinded", imageColor)
    # 先截取灯管的位置,竖直灯关掉额位置,截取掉型材
    # verPos = [0, 2848, 1203, 1728]  # 竖直灯管的位置,ystart,yend,xstart,xend
    # res, upPoint, downPoint = get_ver_light_line(image, verPos, height)
    # if res == "noBoard":
    #     return "noBoard", "竖直方向上没有发现任何的板子遮挡,检测失败", None, None, image
    # elif res == "error":
    #     return "error", "找板失败,按照上一次的位置进行抓板", None, None, image

    # 如果竖直灯管上的位置找到了,找左右灯管的位置.
    # horPos = [200, 1550, 5, 1900]  # 左右灯排的位置yStart,yEnd,xStart,xEnd
    # # 两个灯排的中间位置,用来填充白色
    # midFill = [200, 1550, 750, 1145]
    # res, info, leftLine, rightLine = get_left_right_line(image, horPos, midFill, width)
    # if res != "ok":
    #     return "error", info, None, None, None
    #
    # # 现在是四个轮廓都找到了,对应的点位也找到了.
    # leftLineK, leftLineB = get_line_k_and_b(leftLine[0], leftLine[1])
    # rightLineK, rightLineB = get_line_k_and_b(rightLine[0], rightLine[1])
    #
    # # 获取四个交点.然后根据交点画线.
    # interSection1 = get_ver_line_intersection(leftLineK, leftLineB, upPoint)
    # interSection2 = get_ver_line_intersection(rightLineK, rightLineB, upPoint)
    # interSection3 = get_ver_line_intersection(rightLineK, rightLineB, downPoint)
    # interSection4 = get_ver_line_intersection(leftLineK, leftLineB, downPoint)
    #
    # # 画出轮廓
    # imageColor = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
    # imageColorContour = imageColor.copy()
    #
    # cv.line(imageColorContour, interSection1, interSection2, (0, 255, 0), 3)
    # cv.line(imageColorContour, interSection2, interSection3, (0, 255, 0), 3)
    # cv.line(imageColorContour, interSection3, interSection4, (0, 255, 0,), 3)
    # cv.line(imageColorContour, interSection4, interSection1, (0, 255, 0), 3)
    # image_show("ContourFind", imageColorContour)

    xOffset = int(cp.catchPosOffset6pin * cp.mm2pix)
    # 做右边线的垂线,根据那个补偿值
    fixLineMid = get_two_points_middle(rightLine[0], rightLine[1])
    if rightK is None:
        catchX = fixLineMid[0] - xOffset
        catchY = fixLineMid[1]
    else:
        verK = -1 / rightK
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchX = fixLineMid[0] - abs(xOffset * np.cos(trans_k_to_rad(verK)))
        catchY = verK * catchX + verB

        # 根据左右的斜率和斜距,获取角平分线,求物料角度
    # meanLineK, meanLineB = get_mean_angle(leftLine, rightLine)
    # angle = get_angle_by_k(meanLineK)
    # print("angle", angle)

    # if angle < 0:
    #     angle = round(angle + 90, 2)
    # else:
    #     angle = round(angle - 90, 2)

    catchPos = (int(catchX), int(catchY))
    # 和固定边的交点
    interSection5 = np.int0(fixLineMid).tolist()
    # (x,y)坐标颠倒
    # interSection5 = (interSection5[1],interSection5[0])

    cv.line(imageColor, tuple(interSection5), catchPos, colorRed, lineThickness)
    cv.circle(imageColor, catchPos, 5, colorRed, -1)
    cv.putText(imageColor, "angle:{:.2f}".format(angle), (catchPos[0] - 300, catchPos[1] - 200),
               cv.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
    cv.putText(imageColor, "catchPos:{}".format(catchPos), (catchPos[0] - 300, catchPos[1] - 50), cv.FONT_HERSHEY_SIMPLEX,
               2, (255, 0, 0), 4)
    print("Angle: {:.2f},catchPos:{}".format(angle, catchPos))
    image_show("TotalBoard", imageColor)

    # 根据图像抓取位置,获取实际的坐标偏移
    catchPosRobot = get_module_catch_pos(catchPos)
    if ROBOT_LIMIT_X_MIN < catchPosRobot[0] < ROBOT_LIMIT_X_MAX and ROBOT_LIMIT_Y_MIN < catchPosRobot[1] < ROBOT_LIMIT_Y_Max and \
            abs(angle) < cp.errorAngleThres:
        return "ok", "识别成功!", catchPosRobot, angle, imageColor
    elif abs(angle) >= cp.errorAngleThres:
        return "error", "识别的角度太大,识别角度:{:.2f},阈值:{:.2f}".format(angle, cp.errorAngleThres), catchPosRobot, angle, imageColor
    else:
        info = "识别的坐标超过极限位置,识别的抓取坐标X:{:.2f},ok范围:({},{}),坐标Y:{:.2f},ok范围:({},{})".format(
            catchPosRobot[0], ROBOT_LIMIT_X_MIN, ROBOT_LIMIT_X_MAX, catchPosRobot[1], ROBOT_LIMIT_Y_MIN, ROBOT_LIMIT_Y_Max)
        return "error", info, catchPosRobot, angle, imageColor


def get_module_catch_pos(catchPos):
    # 抓取的图像位置
    catchXpix, catchYPix = catchPos
    # 图像x轴和y轴对应机械手x轴和y轴,只是图像的x轴和机械手的x轴的正负是反的,y轴的正方向和机械手的正方向也是反的.

    imageOffsetX = (catchXpix - cp.centerImageX) / cp.mm2pix  # 转换成mm,坐标值越大的时候,这个偏移是正,到下面要减
    imageOffsetY = (catchYPix - cp.centerImageY) / cp.mm2pix  # 转换成mm,坐标值越大的时候,这个偏移是正,到下面要减

    # 实际的抓板位置
    catchPosX = cp.centerModuleX + imageOffsetX
    catchPosY = cp.centerModuleY - imageOffsetY
    return [int(catchPosX), int(catchPosY)]


def trans_k_to_rad(k):
    """
    将斜率转换为弧度
    :param k: 斜率k值
    :return:
    """
    return np.arctan(k)


def get_angle_by_k(k):
    """
    根据斜率k值,获取对应的角度值
    :param k: 线条的斜率k值
    :return:
    """
    if k == 0:
        return 0
    if k is None:
        return 90
    return np.arctan(k) * 180 / np.pi


def get_mean_angle(leftLine, rightLine):
    """
    获取角平分线
    :param k1:
    :param b1:
    :param k2:
    :param b2:
    :return:
    """
    # 坐标先颠倒,垂直方向的角平分线有点问题
    leftPoints = [(p[1], p[0]) for p in leftLine]
    rightPoints = [(p[1], p[0]) for p in rightLine]
    k1, b1 = get_line_k_and_b(leftPoints[0], leftPoints[1])
    k2, b2 = get_line_k_and_b(rightPoints[0], rightPoints[1])

    intersection = get_two_lines_intersection(k1, b1, k2, b2)
    # 如果本身就是平行线没有交点.这样中线的k就是upK 或者downK,都一样,如果确定b的值呢.
    if intersection[0] is None:
        middleLineK = k1
        y1 = b1
        y2 = b2
        if middleLineK is not None:
            # 先找到中线上的一个必过点
            middleLineP = (0, (y1 + y2) / 2)
            middleLineB = middleLineP[1] - middleLineK * middleLineP[0]
        else:
            middleLineK = None
            middleLineB = (y1 + y2) / 2
    else:
        if k1 is not None and k2 is None:
            middleLineK = np.tan((np.pi / 2 + np.arctan(k1)) / 2)
        elif k1 is None and k2 is not None:
            middleLineK = np.tan((np.pi / 2 + np.arctan(k2)) / 2)
        else:
            middleLineK = np.tan((np.arctan(k1) + np.arctan(k2)) / 2)  # 角平分线公式
        middleLineB = intersection[1] - middleLineK * intersection[0]

    return middleLineK, middleLineB


def get_ver_line_intersection(k1, b1, point):
    """
    获取一个点和一条直线的垂直交点
    :param k1:
    :param b1:
    :param point:
    :return:
    """
    if k1 is None:
        intersection = (int(b1), int(point[1]))
    elif k1 == 0:
        intersectionX = int(point[0])
        intersectionY = int(b1)
        intersection = (intersectionX, intersectionY)
    else:
        verK = -1 / k1
        verB = point[1] - verK * point[0]
        intersection = get_two_lines_intersection(k1, b1, verK, verB)
    return intersection


def distance(p1, p2):
    """
    计算平面上两个点之前的距离,精确到小数点后两位
    :param p1: 平面上的点1
    :param p2: 平面上的点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    return round(np.sqrt((abs(p2[0] - p1[0]) ** 2 + abs(p2[1] - p1[1]) ** 2)), 2)


def get_image_used(image):
    # image = cv.cvtColor(image,cv.COLOR_RGB2GRAY)
    newImage = rotate_image_no_scale(image, -90)  # 逆时针旋转90度.
    # yStart, yEnd = [415, 3249]
    # newImage = newImage[yStart:yEnd,:]
    yStart, yEnd, xStart, xEnd = [440, 3289, 35, 3000]
    newImage = newImage[yStart:yEnd, xStart:xEnd]
    return newImage


if __name__ == '__main__':
    filePath = r"D:\jianye02_plate"
    keepDir = filePath
    if not os.path.isdir(keepDir):
        keepDir, _ = os.path.split(filePath)

    keepOkPath = os.path.join(keepDir, "ok")
    keepOkResPath = os.path.join(keepDir, "res")
    keepFailPath = os.path.join(keepDir, "fail")
    keepFailResPath = os.path.join(keepDir, "fail_res")
    if not os.path.exists(keepOkPath):
        os.makedirs(keepOkPath)
    if not os.path.exists(keepOkResPath):
        os.makedirs(keepOkResPath)
    if not os.path.exists(keepFailPath):
        os.makedirs(keepFailPath)
    if not os.path.exists(keepFailResPath):
        os.makedirs(keepFailResPath)

    width = 1174
    height = 1550
    jointNumber = "6"

    fileNames = []  # 保存要去识别的所有的图片
    if os.path.isfile(filePath):
        filePath, fileName = os.path.split(filePath)
        fileNames.append(fileName)
    else:
        for root, dirs, files in os.walk(filePath):
            if root != filePath:
                continue
            for file in files:
                if file[-4:] != ".bmp":
                    continue
                fileNames.append(file)

    for index, fileName in enumerate(fileNames):
        print("图像名称:{} ".format(fileName))
        filePathReal = os.path.join(filePath, fileName)
        imageTest = cv.imread(filePathReal, cv.IMREAD_GRAYSCALE)
        # imageUsed = get_image_used(imageTest)
        cv.imwrite(os.path.join(keepOkPath, fileName), imageTest)
        calcStart = time.time()
        imageTest = get_image_used(imageTest)
        if jointNumber == "4":
            res, info, center, angle, imageRes = find_total_board_info(imageTest)
        else:
            res, info, center, angle, imageRes = find_total_board_info_6pin(imageTest, width, height)

        if res == "ok":
            print("算法耗时: {}".format(time.time() - calcStart))
            os.remove(filePathReal)
            imageRes = get_size_scale_image(imageRes, sizeScale=4)
            cv.imwrite(os.path.join(keepOkResPath, fileName), imageRes)
            cv.imwrite(os.path.join(keepOkPath, fileName), imageTest)
            print("识别成功")
            continue
        else:
            os.remove(filePathReal)
            cv.imwrite(os.path.join(keepFailPath, fileName), imageTest)
            cv.imwrite(os.path.join(keepFailResPath, fileName), imageRes)
            print("算法耗时: {}".format(time.time() - calcStart))
            print("识别失败:{}".format(info))
